#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist

class OpenLoopController(Node):
    def __init__(self):
        super().__init__('open_loop_controller')
        self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
        self.timer = self.create_timer(0.1, self.timer_callback)
        
        # 定义开环控制序列
        self.motion_sequence = [
            (0.5, 0.0, 2.0),  # 前进0.5m/s，持续2秒
            (0.0, 0.5, 3.0),  # 原地左转0.5rad/s，持续3秒
            (0.5, 0.0, 2.0),  # 前进0.5m/s，持续2秒
            (0.0, -0.5, 3.0)  # 原地右转-0.5rad/s，持续3秒
        ]
        
        self.current_step = 0
        self.start_time = self.get_clock().now()
        
    def timer_callback(self):
        if self.current_step >= len(self.motion_sequence):
            self.get_logger().info("开环控制序列执行完毕")
            self.timer.cancel()
            return
        
        current_time = self.get_clock().now()
        elapsed_time = (current_time - self.start_time).nanoseconds / 1e9
        
        linear_speed, angular_speed, duration = self.motion_sequence[self.current_step]
        
        if elapsed_time >= duration:
            self.current_step += 1
            self.start_time = current_time
            if self.current_step < len(self.motion_sequence):
                self.get_logger().info(f"执行步骤 {self.current_step + 1}/{len(self.motion_sequence)}")
            return
        
        cmd_vel = Twist()
        cmd_vel.linear.x = linear_speed
        cmd_vel.angular.z = angular_speed
        self.cmd_vel_pub.publish(cmd_vel)
        
        self.get_logger().info(f"当前执行步骤 {self.current_step + 1}: "
                              f"线速度={linear_speed:.2f} m/s, 角速度={angular_speed:.2f} rad/s, "
                              f"已执行 {elapsed_time:.1f}/{duration} 秒")

def main(args=None):
    rclpy.init(args=args)
    controller = OpenLoopController()
    rclpy.spin(controller)
    controller.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()